#include "Armor.h"

void Armor::Search_last(vector<Armor_ptr> &last_armors, int frame_number)
{
    double min_distance = 1000000;
    for (auto i : last_armors)
    {
        if (i->hasId)
        {
            if (i->id == this->id)
                continue;
        }

        double let_top_distance = Armor::distance(this->blobs.left->corners[0], i->blobs.left->corners[0]);
        double let_bottom_distance = Armor::distance(this->blobs.left->corners[1], i->blobs.left->corners[1]);
        double rig_top_distance = Armor::distance(this->blobs.left->corners[0], i->blobs.left->corners[0]);
        double rig_bottom_distance = Armor::distance(this->blobs.left->corners[1], i->blobs.left->corners[1]);

        double let_blob_disance = fabs(let_top_distance - let_bottom_distance);
        double rig_blob_disance = fabs(rig_top_distance - rig_bottom_distance);

        double armor_length = (Armor::distance(this->blobs.left->corners[0], this->blobs.right->corners[0]) +
                               Armor::distance(this->blobs.left->corners[1], this->blobs.right->corners[1])) /
                              2;

        double center_distance = Armor::distance(this->armor_center, i->armor_center);

        if (center_distance >= armor_length)
        {
            continue;
        }

        if (let_blob_disance >= 10 || rig_blob_disance >= 10)
        {
            continue;
        }

        if (center_distance <= min_distance)
        {
            this->isMatched = true;
            this->last_armor = i;
            this->numberOf_frame = frame_number;
            if (i->hasId)
            {
                this->id = i->id;
                this->hasId = true;
            }
            if (i->kalmanInited)
            {
                this->KF = i->KF;
                this->kalmanInited = true;
                this->kalmantimes = i->kalmantimes;
            }
        }
    }
    this->updateKlaman();
}

// void Armor::updateKlaman()
// {
//     if (!kalmanInited)
//     {
//         this->KF.init(4, 2, 0);
//         this->KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0,
//                                      0, 1, 0, 1,
//                                      0, 0, 1, 0,
//                                      0, 0, 0, 1);
//         setIdentity(KF.measurementMatrix);
//         this->KF.processNoiseCov = (Mat_<float>(4, 4) << 1e-4, 0, 0, 0,
//                                     0, 1e-6, 0, 0,
//                                     0, 0, 0.01, 0,
//                                     0, 0, 0, 1e-5);
//         setIdentity(KF.measurementNoiseCov, Scalar::all(0.5));
//         setIdentity(KF.errorCovPost, Scalar::all(10)); //后验错误估计协方差矩阵P
//         randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
//         kalmanInited = true;
//     }
//     else
//     {
//         // Mat kalman_point(3,1,CV_32F);
//         // kalman_point.at<float>(0) = armor_center.x;
//         // kalman_point.at<float>(1) = armor_center.y;
//         // kalman_point.at<float>(2) = 1;
//         // float last_yaw, last_picth;

//         Mat measureMatrix0(2, 1, CV_32F);
//         if (this->kalmantimes == 0 || this->last_armor == nullptr)
//         {
//             measureMatrix0.at<float>(0) = 0;
//             measureMatrix0.at<float>(1) = 0;
//             this->kalmantimes++;
//         }
//         else
//         {
//             float delta_time = (this->record_time - this->last_armor->record_time) / getTickFrequency();
//             measureMatrix0.at<float>(0) = (this->relative_yaw - this->last_armor->relative_yaw) / delta_time;
//             measureMatrix0.at<float>(1) = (this->relative_picth - this->last_armor->relative_picth) / delta_time;
//             this->picth_v = measureMatrix0.at<float>(0);
//             this->yaw_v = measureMatrix0.at<float>(1);
//             if (this->picth_v * this->last_armor->picth_v < 0)
//             {
//                 measureMatrix0.at<float>(1) = 0;
//             }
//             if (this->yaw_v * this->last_armor->yaw_v < 0 < 0)
//             {
//                 measureMatrix0.at<float>(0) = 0;
//             }
//         }
//         this->KF.correct(measureMatrix0);
//         Mat predict_mat = this->KF.predict();
//         ofstream k_data("kalman_data.txt", ios::app);
//         float prediction_yaw = predict_mat.at<float>(0); //计算预测值
//         float prediction_picth = predict_mat.at<float>(1);
//         k_data << this->id << endl;
//         k_data << "prediction_yaw:" << prediction_yaw << endl;
//         k_data << "prediction_picth:" << prediction_picth << endl;
//         this->kalman_center.x = this->armor_center.x + tan(prediction_yaw / 180 * 3.14 * setting_time) * 7.2122;
//         this->kalman_center.y = this->armor_center.y + tan(prediction_picth / 180 * 3.14 * setting_time) * 7.2122;
//         k_data << "kalman_center:"
//                << "(" << kalman_center.x << "," << kalman_center.y << ")" << endl;
//         k_data << "armor_center:"
//                << "(" << this->armor_center.x << "," << this->armor_center.y << ")" << endl;
//         if (this->last_armor != nullptr)
//         {
//             k_data << "last_armor_center:"
//                    << "(" << this->last_armor->armor_center.x << "," << this->last_armor->armor_center.y << ")" << endl;
//             float delta_time = (this->record_time - this->last_armor->record_time) / getTickFrequency();
//             k_data << "delta_time:" << delta_time << endl;
//             // k_data << "last_armor_center_time:" << this->last_armor->record_time << endl;
//             // k_data << "this_time:" << this->record_time << endl;
//         }
//         if (fabs(this->kalman_center.x - this->armor_center.x) > 40 || fabs(this->kalman_center.y - this->armor_center.y) > 40)
//         {
//             k_data << "??????????????????????????????????????????????????????????" << endl;
//         }
//         // this->kalman_center.x = this->predicted_dist_x;
//         // this->kalman_center.y = this->predicted_dist_y;
//         this->predicted_dist_x = this->kalman_center.x - 320;
//         this->predicted_dist_y = this->kalman_center.y - 240;
//         k_data << "predicted_dist_x:" << predicted_dist_x << endl;
//         k_data << "predicted_dist_y:" << predicted_dist_y << endl;
//         k_data << "<-------------------------------------------------------->" << endl;
//         k_data.close();
//         if (this->last_armor != nullptr)
//         {
//             if (fabs(this->kalman_center.x - this->last_armor->kalman_center.x) >= 40)
//             {

//                 cout << "this->kalman_center.x:" << this->kalman_center.x << endl;
//                 cout << "this->last_armor->kalman_center.x:" << this->last_armor->kalman_center.x << endl;
//             }
//         }
//     }
// }

void Armor::updateKlaman()
{
    if (!kalmanInited)
    {
        this->KF.init(4, 2, 0);
        this->KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0,
                                     0, 1, 0, 1,
                                     0, 0, 1, 0,
                                     0, 0, 0, 1);
        setIdentity(KF.measurementMatrix);
        this->KF.processNoiseCov = (Mat_<float>(4, 4) << 1e-4, 0, 0, 0,
                                    0, 1e-6, 0, 0,
                                    0, 0, 0.01, 0,
                                    0, 0, 0, 1e-5);
        setIdentity(KF.measurementNoiseCov, Scalar::all(0.5));
        setIdentity(KF.errorCovPost, Scalar::all(10)); //后验错误估计协方差矩阵P
        randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
        kalmanInited = true;
    }
    else
    {
        // Mat kalman_point(3,1,CV_32F);
        // kalman_point.at<float>(0) = armor_center.x;
        // kalman_point.at<float>(1) = armor_center.y;
        // kalman_point.at<float>(2) = 1;
        // float last_yaw, last_picth;

        Mat measureMatrix0(2, 1, CV_32F);
        if (this->kalmantimes == 0 || this->last_armor == nullptr)
        {
            measureMatrix0.at<float>(0) = 0;
            measureMatrix0.at<float>(1) = 0;
            this->kalmantimes++;
        }
        else
        {
            float delta_time = (this->record_time - this->last_armor->record_time) / getTickFrequency();
            measureMatrix0.at<float>(0) = (this->armor_center.x - this->last_armor->armor_center.x) / delta_time;
            measureMatrix0.at<float>(1) = (this->armor_center.y - this->last_armor->armor_center.y) / delta_time;
            this->yaw_v = (this->armor_center.x - this->last_armor->armor_center.x) / delta_time;
            this->picth_v = (this->armor_center.y - this->last_armor->armor_center.y) / delta_time;
            if(this->yaw_v * this->last_armor->yaw_v < 0)
            {
                measureMatrix0.at<float>(0) = 0;
            }
            if(this->picth_v * this->last_armor->picth_v < 0)
            {
                measureMatrix0.at<float>(1) = 0;
            }
        }
        this->KF.correct(measureMatrix0);
        Mat predict_mat = this->KF.predict();
        // ofstream k_data("kalman_data.txt", ios::app);
        float prediction_yaw = predict_mat.at<float>(0); //计算预测值
        float prediction_picth = predict_mat.at<float>(1);
        // k_data << this->id << endl;
        // k_data << "prediction_yaw:" << prediction_yaw << endl;
        // k_data << "prediction_picth:" << prediction_picth << endl;
        this->kalman_center.x = this->armor_center.x + prediction_yaw * setting_time;
        this->kalman_center.y = this->armor_center.y + prediction_picth * setting_time;
        // k_data << "kalman_center:"
        //        << "(" << kalman_center.x << "," << kalman_center.y << ")" << endl;
        // k_data << "armor_center:"
        //        << "(" << this->armor_center.x << "," << this->armor_center.y << ")" << endl;
        // if (this->last_armor != nullptr)
        // {
        //     k_data << "last_armor_center:"
        //            << "(" << this->last_armor->armor_center.x << "," << this->last_armor->armor_center.y << ")" << endl;
        //     float delta_time = (this->record_time - this->last_armor->record_time) / getTickFrequency();
        //     k_data << "delta_time:" << delta_time << endl;
        //     // k_data << "last_armor_center_time:" << this->last_armor->record_time << endl;
        //     // k_data << "this_time:" << this->record_time << endl;
        // }
        // if (fabs(this->kalman_center.x - this->armor_center.x) > 40 || fabs(this->kalman_center.y - this->armor_center.y) > 40)
        // {
        //     k_data << "??????????????????????????????????????????????????????????" << endl;
        // }
        this->predicted_dist_x = this->kalman_center.x - 320;
        this->predicted_dist_y = this->kalman_center.y - 240;
        // k_data << "predicted_dist_x:" << predicted_dist_x << endl;
        // k_data << "predicted_dist_y:" << predicted_dist_y << endl;
        // k_data << "<-------------------------------------------------------->" << endl;
        // k_data.close();
        // if (this->last_armor != nullptr)
        // {
        //     if (fabs(this->kalman_center.x - this->last_armor->kalman_center.x) >= 40)
        //     {

        //         cout << "this->kalman_center.x:" << this->kalman_center.x << endl;
        //         cout << "this->last_armor->kalman_center.x:" << this->last_armor->kalman_center.x << endl;
        //     }
        // }
    }
}

ResultPnP Armor::get_pnpdata(ArmorType type, light_blobs &blob, Mat &cameraMatrix, Mat &distcoeff)
{
    vector<Point2f> img_points;
    Mat rvec, tvec;
    img_points.push_back(blob.left->corners[1]);
    img_points.push_back(blob.left->corners[0]);
    img_points.push_back(blob.right->corners[0]);
    img_points.push_back(blob.right->corners[1]);
    if (type == ArmorType::Small)
    {
        solvePnP(this->small_armor, img_points, cameraMatrix, distcoeff, rvec, tvec, false, SOLVEPNP_IPPE);
    }
    else
    {
        solvePnP(this->big_armor, img_points, cameraMatrix, distcoeff, rvec, tvec, false, SOLVEPNP_IPPE);
    }
    Rodrigues(rvec, rvec);
    rvec.convertTo(rvec, CV_64FC1);
    tvec.convertTo(tvec, CV_64FC1);
    Eigen::Matrix3d Rotated_matrix;
    Eigen::Vector3d Tran_vector;
    // Eigen::Vector3d cameraMatrix_inverse;
    // cv::cv2eigen(cameraMatrix, cameraMatrix_inverse);
    // cameraMatrix_inverse = cameraMatrix_inverse.inverse();
    cv::cv2eigen(rvec, Rotated_matrix);
    cv::cv2eigen(tvec, Tran_vector);
    // Tran_vector = Rotated_matrix * Tran_vector;
    Eigen::Vector3d euler_angles = Rotated_matrix.eulerAngles(0, 1, 2);
    ResultPnP tool_pnp;
    tool_pnp.distance = sqrt(Tran_vector.transpose() * Tran_vector);
    tool_pnp.picth = euler_angles[0];
    tool_pnp.yaw = euler_angles[1];
    tool_pnp.roll = euler_angles[2];
    // this->relative_yaw = atan(Tran_vector[0] / sqrt(pow(Tran_vector[1], 2) + pow(Tran_vector[2], 2))) / 3.14159 * 180;
    // this->relative_picth = atan(-Tran_vector[1] / Tran_vector[2]) / 3.14159 * 180;
    this->relative_yaw = atan(Tran_vector[0 / Tran_vector[2]]) / 3.14159 * 180;
    this->relative_picth = atan(Tran_vector[1] / Tran_vector[2]) / 3.14159 * 180;
    return tool_pnp;
}

void Armor::get_center()
{
    //     this->armor_center.x = (this->blobs.left->corners[0].x + this->blobs.right->corners[1].x) / 2;
    //     this->armor_center.y = (this->blobs.left->corners[0].y + this->blobs.right->corners[1].y) / 2;
    int x, y;
    int X1 = this->blobs.left->corners[0].x - this->blobs.right->corners[1].x, Y1 = this->blobs.left->corners[0].y - this->blobs.right->corners[1].y, X2 = this->blobs.left->corners[1].x - this->blobs.right->corners[0].x, Y2 = this->blobs.left->corners[1].y - this->blobs.right->corners[0].y;

    if (X1 * Y2 == X2 * Y1)
    {
        this->armor_center.x = (this->blobs.left->corners[1].x - this->blobs.right->corners[0].x) / 2;
        this->armor_center.y = (this->blobs.left->corners[1].y - this->blobs.right->corners[0].y) / 2;
        return;
    }

    int A = X1 * this->blobs.left->corners[0].y - Y1 * this->blobs.left->corners[0].x, B = X2 * this->blobs.left->corners[1].y - Y2 * this->blobs.left->corners[1].x;
    y = (A * Y2 - B * Y1) / (X1 * Y2 - X2 * Y1);
    x = (B * X1 - A * X2) / (Y1 * X2 - Y2 * X1);
    this->armor_center.x = x;
    this->armor_center.y = y;
}

double Armor::distance(vector<Point> &blob)
{
    return sqrt(pow(blob[0].x - blob[1].x, 2) + pow(blob[0].y - blob[1].y, 2));
}

double Armor::distance(Point a, Point b)
{
    return sqrt(pow(a.x - b.x, 2) + pow(a.y - b.y, 2));
}

void Armor::get_center_distance()
{
    this->center_diatance_x = this->armor_center.x - 320;
    this->center_diatance_y = this->armor_center.y - 240;
}
